#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/pwm.h"

#include "smooth.h"

#include "video_dataP.h"

//Imaging macros
#define WIDTH 45 
#define HEIGHT 29
#define NUM_PIXELS (WIDTH * HEIGHT)
#define DAC_BITS 6
#define MAX_DAC_VALUE ((1 << DAC_BITS) - 1)
#define FRAME_DURATION_US 1000000/10 // 1/30 sekundy
#define PIXEL_DURATION_US (FRAME_DURATION_US / NUM_PIXELS)  // time for 1 pixel

//driving macros
#define SENSOR_PIN 22
#define PWM_PIN 28
#define SECOND 1000000
#define SEC_PER_MIN 60
#define PWM_WRAP 1999

volatile uint64_t current_period_us = FRAME_DURATION_US;
volatile float current_rpm = 0.0f;
float integral = 0, last_error;
static exp_avg_t pixel_time_filter;
static adaptive_avg_t period_filter;

// PID constants
#define TARGET_RPM 3000.0f
#define KP 0.05f
#define KI 0.01f
#define KD 0.005f


//dac pins
const uint RED_PINS[DAC_BITS]   = {15, 14, 13, 12, 11, 10};
const uint GREEN_PINS[DAC_BITS] = {4, 5, 6, 7, 8, 9};
const uint BLUE_PINS[DAC_BITS]  = {16, 17, 18, 19, 20, 21};

//user interface
const uint ON_PIN = 26;

void init_dac_pins(const uint pins[]) {
    for (int i = 0; i < DAC_BITS; i++) {
        gpio_init(pins[i]);
        gpio_set_dir(pins[i], GPIO_OUT);
    }
}

void set_dac_channel(const uint pins[], uint8_t value) {
    for (int i = 0; i < DAC_BITS; i++) {
        gpio_put(pins[i], (value >> i) & 1);
    }
}

void set_rgb(uint8_t r, uint8_t g, uint8_t b) {
    set_dac_channel(RED_PINS, r);
    set_dac_channel(GREEN_PINS, g);
    set_dac_channel(BLUE_PINS, b);
}

// Initialize PWM and return slice number
uint setup_pwm() {
    gpio_set_function(PWM_PIN, GPIO_FUNC_PWM);
    uint slice = pwm_gpio_to_slice_num(PWM_PIN);
    pwm_set_clkdiv(slice, 125.0f);       // 1 MHz clock
    pwm_set_wrap(slice, PWM_WRAP);       // 500Hz PWM bcs high impedance motor
    pwm_set_enabled(slice, true);
    return slice;
}

// Set PWM level safely
void update_pwm(uint level) {
    if (level > PWM_WRAP) level = PWM_WRAP;
    pwm_set_gpio_level(PWM_PIN, level);
}

// Compute PID based on current RPM and return PWM level
uint compute_pid(float rpm) {
    float error = TARGET_RPM - rpm;
    integral += error;
    float derivative = error - last_error;
    float output = KP * error + KI * integral + KD * derivative;
    last_error = error;

    // Scale and clamp to PWM level
    int pwm_level = (int)output;
    if (pwm_level < 0) pwm_level = 0;
    if (pwm_level > PWM_WRAP) pwm_level = PWM_WRAP;
    return (uint)pwm_level;
}

// Core 1 thread: RPM measurement and PID control
void rpm_pid_control_thread() {
    gpio_init(SENSOR_PIN);
    gpio_set_dir(SENSOR_PIN, GPIO_IN);
    gpio_pull_down(SENSOR_PIN);

    uint pwm_slice = setup_pwm();

    uint64_t last_time = 0;
    bool last_state = false;

    while (true) {
        bool current_state = gpio_get(SENSOR_PIN);
        uint64_t current_time = time_us_64();

        if (!last_state && current_state) {
            if (last_time != 0) {
                float period_us = (float)(current_time - last_time);
                uint64_t filtered_period = adaptive_avg_update(&period_filter, period_us);
                current_period_us = filtered_period;
                float frequency = SECOND / period_us;
                current_rpm = frequency * SEC_PER_MIN;

                //uint pwm_level = compute_pid(current_rpm);
                //update_pwm(pwm_level); 
                printf("[Core 1] RPM = %.2f\n", current_rpm);
            }
            last_time = current_time;
        }

        last_state = current_state;
        sleep_us(10);
        //if(current_time%100 == 0) {}
    }
}

int main() {
    stdio_init_all();
    sleep_ms(500);
    gpio_init(ON_PIN);
    gpio_set_dir(ON_PIN, GPIO_IN);
    gpio_pull_up(ON_PIN);

    multicore_launch_core1(rpm_pid_control_thread);

    init_dac_pins(RED_PINS);
    init_dac_pins(GREEN_PINS);
    init_dac_pins(BLUE_PINS);

    while (true) {
        if (gpio_get(ON_PIN)) {
            uint64_t raw_pixel_time = current_period_us / NUM_PIXELS;
            uint64_t stable_pixel_time = exp_avg_update(&pixel_time_filter, raw_pixel_time);

            for (int frame = 0; frame < NUM_FRAMES; frame++) {
                absolute_time_t start_time = get_absolute_time();
                const uint8_t *image = video_data[frame];

                for (int i = 0; i < NUM_PIXELS; i++) {
                    int idx = i * 3;
                    uint8_t r = image[idx];
                    uint8_t g = image[idx + 1];
                    uint8_t b = image[idx + 2];

                    set_rgb(r, g, b);
                    absolute_time_t next_time = delayed_by_us(start_time, (i + 1) * stable_pixel_time);
                    sleep_until(next_time);
                }
            }
        } else {
            set_rgb(0, 0, 0);
        }
    }
    return 0;
}

